#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
传感器管理模块 - Sensor Manager Module

该模块负责管理机器人的各种传感器，包括：
- 相机控制和图像捕获
- 雷达数据获取和处理
- 位置传感器和地图定位
- ROS2节点和话题状态检查

This module manages various robot sensors including:
- Camera control and image capture
- Lidar data acquisition and processing
- Position sensors and map localization
- ROS2 node and topic status checking
"""

import time
import subprocess
import os
from typing import Optional, Union, Dict, Any


class SensorManager:
    """
    传感器管理器类
    Sensor Manager Class
    
    负责管理机器人的传感器系统，包括相机、雷达和位置传感器
    Manages robot sensor systems including camera, lidar, and position sensors
    """
    
    def __init__(self, muto_instance=None):
        """
        初始化传感器管理器
        Initialize sensor manager
        
        Args:
            muto_instance: Muto机器人实例 Muto robot instance
        """
        self.muto = muto_instance
    
    # ==================== ROS2节点和话题检查 ROS2 Node and Topic Checking ====================
    
    def check_ros2_node_running(self, node_name: str, namespace: str = "/") -> tuple:
        """
        检查ROS2节点是否正在运行
        Check if ROS2 node is running
        
        Args:
            node_name: 节点名称 Node name
            namespace: 命名空间 Namespace (default: "/")
            
        Returns:
            tuple: (success: bool, message: str, data: bool)
                success: 检查是否成功 Whether check was successful
                message: 检查结果消息 Check result message
                data: 节点是否运行 Whether node is running
        
        Examples:
            success, msg, is_running = sensor_manager.check_ros2_node_running("camera_node")
        """
        try:
            # 获取所有运行中的节点
            result = subprocess.run(
                ["ros2", "node", "list"],
                capture_output=True,
                text=True,
                timeout=10
            )
            
            if result.returncode == 0:
                nodes = result.stdout.strip().split('\n')
                # 构建完整节点名称
                full_node_name = f"{namespace.rstrip('/')}/{node_name}" if namespace != "/" else f"/{node_name}"
                
                # 检查节点是否在列表中
                for node in nodes:
                    if node.strip() == full_node_name:
                        print(f"✅ ROS2节点运行中 Node running: {full_node_name}")
                        return True, f"ROS2节点{full_node_name}正在运行 ROS2 node {full_node_name} is running", True
                
                print(f"❌ ROS2节点未运行 Node not running: {full_node_name}")
                print(f"当前运行的节点 Current running nodes: {nodes}")
                return True, f"ROS2节点{full_node_name}未运行 ROS2 node {full_node_name} is not running", False
            else:
                print(f"❌ 检查ROS2节点失败 Failed to check ROS2 nodes: {result.stderr}")
                return False, f"检查ROS2节点失败: {result.stderr} Failed to check ROS2 nodes: {result.stderr}", False
                
        except subprocess.TimeoutExpired:
            print("⏰ 检查ROS2节点超时 ROS2 node check timeout")
            return False, "检查ROS2节点超时 ROS2 node check timeout", False
        except FileNotFoundError:
            print("❌ ROS2未安装或未在PATH中 ROS2 not installed or not in PATH")
            return False, "ROS2未安装或未在PATH中 ROS2 not installed or not in PATH", False
        except Exception as e:
            print(f"❌ 检查ROS2节点出错 Error checking ROS2 node: {e}")
            return False, f"检查ROS2节点出错: {str(e)} Error checking ROS2 node: {str(e)}", False
    
    def check_ros2_topic_active(self, topic_name: str, expected_type: str = "sensor_msgs/msg/Image") -> tuple:
        """
        检查ROS2话题是否正确发布
        Check if ROS2 topic is actively publishing
        
        Args:
            topic_name: 话题名称 Topic name
            expected_type: 期望的消息类型 Expected message type
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 执行是否成功 Whether execution was successful
                message: 执行结果消息 Execution result message
                data: 话题状态信息 Topic status information
        """
        try:
            # 参数有效性检查
            if not topic_name or not isinstance(topic_name, str):
                return False, "话题名称无效 Invalid topic name", {"topic_name": topic_name, "expected_type": expected_type, "active": False}
            
            if not expected_type or not isinstance(expected_type, str):
                return False, "期望消息类型无效 Invalid expected message type", {"topic_name": topic_name, "expected_type": expected_type, "active": False}
            
            # 检查话题信息
            result = subprocess.run(
                ["ros2", "topic", "info", topic_name, "--verbose"],
                capture_output=True,
                text=True,
                timeout=10
            )
            
            if result.returncode == 0:
                output = result.stdout
                
                # 检查消息类型
                if f"Type: {expected_type}" in output:
                    # 检查发布者数量
                    if "Publisher count: 1" in output or "Publisher count: " in output:
                        # 提取发布者数量
                        for line in output.split('\n'):
                            if "Publisher count:" in line:
                                count = int(line.split(":")[1].strip())
                                if count > 0:
                                    print(f"✅ ROS2话题正常发布 Topic actively publishing: {topic_name}")
                                    print(f"   消息类型 Message type: {expected_type}")
                                    print(f"   发布者数量 Publisher count: {count}")
                                    return True, f"ROS2话题正常发布 Topic actively publishing: {topic_name}", {"topic_name": topic_name, "expected_type": expected_type, "active": True, "publisher_count": count}
                                else:
                                    print(f"❌ ROS2话题无发布者 No publishers for topic: {topic_name}")
                                    return False, f"ROS2话题无发布者 No publishers for topic: {topic_name}", {"topic_name": topic_name, "expected_type": expected_type, "active": False, "publisher_count": 0}
                    else:
                        print(f"❌ ROS2话题发布者信息异常 Abnormal publisher info for topic: {topic_name}")
                        return False, f"ROS2话题发布者信息异常 Abnormal publisher info for topic: {topic_name}", {"topic_name": topic_name, "expected_type": expected_type, "active": False, "error": "abnormal_publisher_info"}
                else:
                    print(f"❌ ROS2话题消息类型不匹配 Topic message type mismatch: {topic_name}")
                    print(f"   期望类型 Expected: {expected_type}")
                    return False, f"ROS2话题消息类型不匹配 Topic message type mismatch: {topic_name}", {"topic_name": topic_name, "expected_type": expected_type, "active": False, "error": "type_mismatch"}
            else:
                print(f"❌ 检查ROS2话题失败 Failed to check ROS2 topic: {result.stderr}")
                return False, f"检查ROS2话题失败 Failed to check ROS2 topic: {result.stderr}", {"topic_name": topic_name, "expected_type": expected_type, "active": False, "error": result.stderr}
                
        except subprocess.TimeoutExpired:
            print("⏰ 检查ROS2话题超时 ROS2 topic check timeout")
            return False, "检查ROS2话题超时 ROS2 topic check timeout", {"topic_name": topic_name, "expected_type": expected_type, "active": False, "error": "timeout"}
        except FileNotFoundError:
            print("❌ ROS2未安装或未在PATH中 ROS2 not installed or not in PATH")
            return False, "ROS2未安装或未在PATH中 ROS2 not installed or not in PATH", {"topic_name": topic_name, "expected_type": expected_type, "active": False, "error": "ros2_not_found"}
        except Exception as e:
            print(f"❌ 检查ROS2话题出错 Error checking ROS2 topic: {e}")
            return False, f"检查ROS2话题出错 Error checking ROS2 topic: {str(e)}", {"topic_name": topic_name, "expected_type": expected_type, "active": False, "error": str(e)}
    
    # ==================== 相机控制 Camera Control ====================
    
    def start_camera(self, check_before_start: bool = True, wait_time_s: float = 5.0) -> tuple:
        """
        启动相机
        Start camera
        
        Args:
            check_before_start: 启动前是否检查状态 Whether to check status before starting
            wait_time_s: 启动后等待时间(秒) Wait time after starting in seconds
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 执行是否成功 Whether execution was successful
                message: 执行结果消息 Execution result message
                data: 相机启动状态信息 Camera startup status information
        """
        camera_node = "camera"
        camera_namespace = "/camera"
        image_topic = "/camera/color/image_raw"
        
        try:
            # 如果需要，先检查相机状态
            if check_before_start:
                # 检查节点是否已经运行
                node_success, node_msg, node_running = self.check_ros2_node_running(camera_node, camera_namespace)
                if node_success and node_running:
                    # 检查话题是否正常
                    topic_success, topic_msg, topic_data = self.check_ros2_topic_active(image_topic)
                    if topic_success and topic_data.get('active', False):
                        # 相机已经正常运行 Camera is already running normally
                        return True, "相机已经正常运行 Camera is already running normally", {"status": "already_running", "node_running": True, "topic_active": True}
            
            # 启动相机
            # 使用subprocess.Popen启动相机（非阻塞）
            process = subprocess.Popen(
                ["ros2", "launch", "astra_camera", "astro_pro_plus.launch.xml"],
                stdout=subprocess.PIPE,
                stderr=subprocess.PIPE,
                text=True
            )
            
            # 等待一段时间让相机启动
            time.sleep(wait_time_s)
            
            # 检查进程是否还在运行
            if process.poll() is None:
                # 验证相机是否正常工作（带重试机制）
                # 设置重试参数
                max_retries = 5  # 最大重试次数
                retry_interval = 2.0  # 重试间隔(秒)
                
                node_running = False
                topic_active = False
                
                for attempt in range(max_retries):
                    if attempt > 0:
                        time.sleep(retry_interval)
                    
                    # 检查节点
                    node_success, node_msg, node_running = self.check_ros2_node_running(camera_node, camera_namespace)
                    
                    # 检查话题
                    if node_success and node_running:
                        topic_success, topic_msg, topic_data = self.check_ros2_topic_active(image_topic)
                        topic_active = topic_success and topic_data.get('active', False)
                        
                        if topic_active:
                            break
                
                return True, "相机启动成功 Camera started successfully", {
                    "process_id": process.pid,
                    "node_running": node_running,
                    "topic_active": topic_active,
                    "camera_node": camera_node,
                    "image_topic": image_topic
                }
            else:
                # 进程已退出
                stdout, stderr = process.communicate()
                return False, f"相机启动失败 Camera startup failed: {stderr}", {
                    "process_id": process.pid,
                    "exit_code": process.returncode,
                    "stderr": stderr,
                    "stdout": stdout
                }
                
        except FileNotFoundError:
            return False, "ROS2或相机驱动包未找到 ROS2 or camera driver package not found", {"error": "ros2_or_camera_not_found"}
        except Exception as e:
            return False, f"启动相机出错 Error starting camera: {str(e)}", {"error": str(e)}
    
    def get_camera_status(self) -> tuple:
        """
        获取相机状态
        Get camera status
        
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 获取是否成功 Whether get was successful
                message: 获取结果消息 Get result message
                data: 相机状态信息 Camera status information
        """
        try:
            camera_node = "camera"
            camera_namespace = "/camera"
            image_topic = "/camera/color/image_raw"
            
            # 检查相机节点状态
            node_success, node_msg, node_running = self.check_ros2_node_running(camera_node, camera_namespace)
            
            # 检查图像话题状态
            topic_success, topic_msg, topic_data = self.check_ros2_topic_active(image_topic)
            topic_active = topic_success and topic_data.get('active', False)
            
            status = "running" if (node_running and topic_active) else "stopped"
            
            return True, f"相机状态: {status} Camera status: {status}", {
                "status": status,
                "camera_node_running": node_running,
                "color_topic_active": topic_active,
                "depth_topic_active": False,  # 暂时设为False，后续可扩展
                "overall_status": "normal" if (node_running and topic_active) else "offline",
                "detected_topics": [image_topic] if topic_active else [],
                "detected_nodes": [f"{camera_namespace}/{camera_node}"] if node_running else [],
                "camera_node": camera_node,
                "image_topic": image_topic,
                "node_check": {"success": node_success, "message": node_msg},
                "topic_check": {"success": topic_success, "message": topic_msg, "data": topic_data}
            }
        except Exception as e:
            return False, f"获取相机状态出错 Error getting camera status: {str(e)}", {"error": str(e)}
    
    def check_camera_status(self) -> tuple:
        """
        检查相机状态（别名方法）
        Check camera status (alias method)
        
        Returns:
            tuple: (success: bool, message: str, data: dict)
        """
        return self.get_camera_status()
    
    def stop_camera(self, force_kill: bool = False, wait_time_s: float = 3.0) -> tuple:
        """
        停止相机
        Stop camera
        
        Args:
            force_kill: 是否强制终止 Whether to force kill
            wait_time_s: 等待时间(秒) Wait time in seconds
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 停止是否成功 Whether stop was successful
                message: 停止结果消息 Stop result message
                data: 停止状态信息 Stop status information
        """
        try:
            camera_node = "camera"
            camera_namespace = "/camera"
            
            # 检查相机是否在运行
            node_success, node_msg, node_running = self.check_ros2_node_running(camera_node, camera_namespace)
            
            if not node_running:
                return True, "相机已经停止 Camera already stopped", {"status": "already_stopped", "node_running": False}
            
            # 尝试优雅地停止相机节点
            if not force_kill:
                try:
                    # 使用ros2 lifecycle命令停止节点（如果支持）
                    result = subprocess.run(
                        ["ros2", "lifecycle", "set", f"{camera_namespace}/{camera_node}", "shutdown"],
                        capture_output=True,
                        text=True,
                        timeout=wait_time_s
                    )
                    
                    if result.returncode == 0:
                        time.sleep(1.0)  # 等待节点停止
                        # 再次检查节点状态
                        node_success, node_msg, node_running = self.check_ros2_node_running(camera_node, camera_namespace)
                        if not node_running:
                            return True, "相机优雅停止成功 Camera gracefully stopped", {"method": "lifecycle", "node_running": False}
                except (subprocess.TimeoutExpired, FileNotFoundError):
                    pass  # 如果lifecycle命令不可用，继续尝试其他方法
            
            # 强制终止相机进程
            try:
                # 使用pkill命令终止相机进程
                result = subprocess.run(
                    ["pkill", "-f", "astra_camera"],
                    capture_output=True,
                    text=True,
                    timeout=5.0
                )
                
                time.sleep(wait_time_s)
                
                # 检查节点是否已停止
                node_success, node_msg, node_running = self.check_ros2_node_running(camera_node, camera_namespace)
                
                if not node_running:
                    return True, "相机强制停止成功 Camera force stopped", {"method": "pkill", "node_running": False}
                else:
                    # 即使进程终止命令执行了，但节点检查显示仍在运行，可能是检查延迟
                    # 再等待一段时间后重新检查
                    time.sleep(2.0)
                    node_success, node_msg, node_running = self.check_ros2_node_running(camera_node, camera_namespace)
                    
                    if not node_running:
                        return True, "相机延迟停止成功 Camera stopped with delay", {"method": "pkill", "node_running": False, "delayed": True}
                    else:
                        return False, "相机停止失败 Failed to stop camera", {"method": "pkill", "node_running": True, "error": "still_running"}
                    
            except Exception as e:
                return False, f"停止相机出错 Error stopping camera: {str(e)}", {"error": str(e)}
                
        except Exception as e:
            return False, f"停止相机出错 Error stopping camera: {str(e)}", {"error": str(e)}
    
    def capture_image(self, save_path: str = None, timeout_s: float = 5.0, check_camera_first: bool = True, image_format: str = "jpg") -> tuple:
        """
        捕获图像
        Capture image
        
        Args:
            save_path: 保存路径 Save path
            timeout_s: 超时时间(秒) Timeout in seconds
            check_camera_first: 是否先检查相机状态 Whether to check camera status first
            image_format: 图像格式 Image format
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 捕获是否成功 Whether capture was successful
                message: 捕获结果消息 Capture result message
                data: 捕获状态信息 Capture status information
        """
        try:
            # 如果需要，先检查相机状态
            if check_camera_first:
                camera_success, camera_msg, camera_data = self.get_camera_status()
                if not camera_success or camera_data.get('status') != 'running':
                    return False, f"相机未运行 Camera not running: {camera_msg}", {"camera_status": camera_data}
            
            # 设置默认保存路径
            if save_path is None:
                timestamp = int(time.time())
                save_path = f"captured_image_{timestamp}.{image_format}"
            
            # 使用ros2 topic echo保存图像
            image_topic = "/camera/color/image_raw"
            
            try:
                # 尝试使用cv_bridge捕获图像 Trying to capture image using cv_bridge
                
                # 检查必要的依赖
                try:
                    import rclpy
                    from rclpy.node import Node
                    from sensor_msgs.msg import Image
                    from cv_bridge import CvBridge
                    import cv2
                    import numpy as np
                    # 所有必要依赖可用 All required dependencies available
                except ImportError as import_error:
                    # 缺少必要依赖 Missing required dependencies
                    # 请安装: pip install opencv-python
                    # Please install: pip install opencv-python
                    return self._capture_image_fallback(save_path, image_topic, timeout_s, image_format)
                
                # 创建图像捕获节点
                class ImageCaptureNode(Node):
                    def __init__(self):
                        super().__init__('image_capture_node')
                        self.bridge = CvBridge()
                        self.image_received = False
                        self.captured_image = None
                        self.subscription = self.create_subscription(
                            Image,
                            image_topic,
                            self.image_callback,
                            10
                        )
                        # 已订阅图像话题 Subscribed to image topic
                    
                    def image_callback(self, msg):
                        try:
                            # 将ROS图像消息转换为OpenCV图像
                            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
                            self.captured_image = cv_image
                            self.image_received = True
                            # 成功接收图像 Successfully received image
                            pass
                        except Exception as e:
                            # 图像转换失败 Image conversion failed
                            pass
                
                # 初始化ROS2（如果尚未初始化）
                if not rclpy.ok():
                    rclpy.init()
                
                # 创建节点并等待图像
                node = ImageCaptureNode()
                
                # 等待图像数据 Waiting for image data
                
                # 等待图像数据
                start_time = time.time()
                while not node.image_received and (time.time() - start_time) < timeout_s:
                    rclpy.spin_once(node, timeout_sec=0.1)
                
                if node.image_received and node.captured_image is not None:
                    # 保存图像
                    success_save = cv2.imwrite(save_path, node.captured_image)
                    
                    if success_save:
                        # 获取图像信息
                        height, width, channels = node.captured_image.shape
                        file_size = os.path.getsize(save_path)
                        
                        # 图像捕获成功 Image captured successfully
                        # 分辨率 Resolution
                        # 通道数 Channels
                        # 文件大小 File size
                        # 保存路径 Saved to
                        
                        # 清理节点
                        node.destroy_node()
                        
                        return True, "图像捕获成功 Image captured successfully", {
                            "save_path": save_path,
                            "format": image_format,
                            "resolution": f"{width}x{height}",
                            "channels": channels,
                            "file_size": file_size,
                            "method": "cv_bridge"
                        }
                    else:
                        # 图像保存失败 Failed to save image to
                        node.destroy_node()
                        return False, f"图像保存失败 Failed to save image to: {save_path}", {"save_path": save_path, "error": "save_failed"}
                else:
                    # 超时或未接收到图像 Timeout or no image received
                    node.destroy_node()
                    return self._capture_image_fallback(save_path, image_topic, timeout_s, image_format)
                
            except Exception as e:
                return False, f"捕获图像出错 Error capturing image: {str(e)}", {"save_path": save_path, "error": str(e)}
                
        except Exception as e:
            return False, f"捕获图像出错 Error capturing image: {str(e)}", {"save_path": save_path, "error": str(e)}
    
    def _capture_image_fallback(self, save_path, image_topic, timeout_s, image_format):
        """
        备用图像捕获方法，使用cv_bridge进行图像转换
        Fallback image capture method using cv_bridge for image conversion
        """
        try:
            # 检查必要的依赖
            try:
                import rclpy
                from rclpy.node import Node
                from sensor_msgs.msg import Image
                from cv_bridge import CvBridge
                import cv2
                # 所有必要依赖可用 All required dependencies available
            except ImportError as import_error:
                # 缺少必要依赖 Missing required dependencies
                # 请安装: pip install opencv-python
                # Please install: pip install opencv-python
                return False, f"缺少必要依赖 Missing required dependencies: {str(import_error)}", {"save_path": save_path, "error": "missing_dependencies"}
            
            # 创建图像捕获节点（备用方法）
            class FallbackImageCaptureNode(Node):
                def __init__(self):
                    super().__init__('fallback_image_capture_node')
                    self.bridge = CvBridge()
                    self.image_received = False
                    self.captured_image = None
                    self.subscription = self.create_subscription(
                        Image,
                        image_topic,
                        self.image_callback,
                        10
                    )
                    # 已订阅图像话题（备用方法） Subscribed to image topic (fallback method)
                
                def image_callback(self, msg):
                    try:
                        # 将ROS图像消息转换为OpenCV图像
                        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
                        self.captured_image = cv_image
                        self.image_received = True
                        # 成功接收图像（备用方法） Successfully received image (fallback method)
                    except Exception as e:
                        print(f"图像转换失败（备用方法） Image conversion failed (fallback method): {str(e)}")
                        # 图像转换失败（备用方法） Image conversion failed (fallback method)
            
            # 初始化ROS2（如果尚未初始化）
            if not rclpy.ok():
                rclpy.init()
            
            # 创建节点并等待图像
            node = FallbackImageCaptureNode()
            
            # 等待图像数据（备用方法） Waiting for image data (fallback method)
            
            # 等待图像数据
            start_time = time.time()
            while not node.image_received and (time.time() - start_time) < timeout_s:
                rclpy.spin_once(node, timeout_sec=0.1)
            
            if node.image_received and node.captured_image is not None:
                # 保存图像
                success_save = cv2.imwrite(save_path, node.captured_image)
                
                if success_save:
                    # 获取图像信息
                    height, width, channels = node.captured_image.shape
                    file_size = os.path.getsize(save_path)
                    
                    # 图像捕获成功（备用方法） Image captured successfully (fallback method)
                    # 分辨率 Resolution
                    # 通道数 Channels
                    # 文件大小 File size
                    # 保存路径 Saved to
                    
                    # 清理节点
                    node.destroy_node()
                    
                    return True, "图像捕获成功（备用方法） Image captured successfully (fallback method)", {
                        "save_path": save_path,
                        "format": image_format,
                        "resolution": f"{width}x{height}",
                        "channels": channels,
                        "file_size": file_size,
                        "method": "cv_bridge_fallback"
                    }
                else:
                    # 图像保存失败（备用方法） Failed to save image (fallback method)
                    node.destroy_node()
                    return False, f"图像保存失败（备用方法） Failed to save image (fallback method): {save_path}", {"save_path": save_path, "error": "save_failed"}
            else:
                # 超时或未接收到图像（备用方法） Timeout or no image received (fallback method)
                # 请检查图像话题是否正常发布数据 Please check if image topic is publishing data normally
                node.destroy_node()
                return False, f"图像捕获超时（备用方法） Image capture timeout (fallback method) ({timeout_s}s)", {
                    "save_path": save_path, 
                    "timeout": timeout_s, 
                    "suggestion": "check_image_topic_publishing"
                }
                
        except Exception as fallback_error:
            # 备用方法失败 Fallback method failed
            return False, f"备用方法失败 Fallback method failed: {str(fallback_error)}", {
                "save_path": save_path,
                "error": str(fallback_error)
            }
    
    def get_depth_at_point(self, x: int, y: int, timeout_s: float = 5.0) -> tuple:
        """
        获取指定坐标点的深度信息
        Get depth information at specified coordinates
        
        Args:
            x: 图像中的x坐标 X coordinate in the image
            y: 图像中的y坐标 Y coordinate in the image
            timeout_s: 超时时间(秒) Timeout in seconds
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 执行是否成功 Whether execution was successful
                message: 执行结果消息 Execution result message
                data: 深度信息 Depth information
        """
        depth_topic = "/camera/depth/image_raw"
        
        try:
            # 开始获取坐标点的深度信息 Starting to get depth info at point
            # 深度话题 Depth topic
            
            # 检查必要的依赖
            try:
                import rclpy
                from rclpy.node import Node
                from sensor_msgs.msg import Image
                from cv_bridge import CvBridge
                import cv2
                import numpy as np
                # 所有必要依赖可用 All required dependencies available
            except ImportError as import_error:
                # 缺少必要依赖 Missing required dependencies
                # 请安装: pip install opencv-python
                # Please install: pip install opencv-python
                return False, f"缺少必要依赖 Missing required dependencies: {str(import_error)}", {"x": x, "y": y, "error": "missing_dependencies"}
            
            # 创建深度图像捕获节点
            class DepthCaptureNode(Node):
                def __init__(self):
                    super().__init__('depth_capture_node')
                    self.bridge = CvBridge()
                    self.depth_received = False
                    self.depth_image = None
                    self.subscription = self.create_subscription(
                        Image,
                        depth_topic,
                        self.depth_callback,
                        10
                    )
                    # 已订阅深度话题 Subscribed to depth topic
                
                def depth_callback(self, msg):
                    try:
                        # 将ROS深度图像消息转换为OpenCV图像
                        # 深度图像通常是16位单通道图像
                        depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")
                        self.depth_image = depth_image
                        self.depth_received = True
                        # 成功接收深度图像 Successfully received depth image
                    except Exception as e:
                        print(f"深度图像转换失败 Depth image conversion failed: {str(e)}")
                        # 深度图像转换失败 Depth image conversion failed
            
            # 初始化ROS2（如果尚未初始化）
            if not rclpy.ok():
                rclpy.init()
            
            # 创建节点并等待深度图像
            node = DepthCaptureNode()
            
            # 等待深度图像数据 Waiting for depth image data
            
            # 等待深度图像数据
            start_time = time.time()
            while not node.depth_received and (time.time() - start_time) < timeout_s:
                rclpy.spin_once(node, timeout_sec=0.1)
            
            if node.depth_received and node.depth_image is not None:
                # 检查坐标是否在图像范围内
                height, width = node.depth_image.shape[:2]
                
                if x < 0 or x >= width or y < 0 or y >= height:
                    # 坐标超出图像范围 Coordinates out of image bounds
                    # 图像尺寸 Image size
                    # 请求坐标 Requested coordinates
                    node.destroy_node()
                    return False, f"坐标超出图像范围 Coordinates out of bounds: ({x}, {y}), image size: {width}x{height}", {
                        "x": x, "y": y, 
                        "image_width": width, 
                        "image_height": height,
                        "error": "coordinates_out_of_bounds"
                    }
                
                # 获取指定坐标点的深度值
                depth_value = node.depth_image[y, x]
                
                # 深度值通常以毫米为单位，转换为米
                if node.depth_image.dtype == np.uint16:
                    # 16位深度图像，通常以毫米为单位
                    depth_meters = float(depth_value) / 1000.0
                    depth_mm = float(depth_value)
                elif node.depth_image.dtype == np.float32:
                    # 32位浮点深度图像，通常以米为单位
                    depth_meters = float(depth_value)
                    depth_mm = float(depth_value) * 1000.0
                else:
                    # 其他格式，假设以毫米为单位
                    depth_meters = float(depth_value) / 1000.0
                    depth_mm = float(depth_value)
                
                # 检查深度值是否有效（0通常表示无效深度）
                is_valid = depth_value > 0
                
                # 深度信息获取成功 Depth information retrieved successfully
                # 坐标 Coordinates
                # 原始深度值 Raw depth value
                # 深度(米) Depth (meters)
                # 深度(毫米) Depth (millimeters)
                # 数据类型 Data type
                # 有效性 Valid
                
                # 获取周围区域的深度信息（3x3区域）
                surrounding_depths = []
                for dy in range(-1, 2):
                    for dx in range(-1, 2):
                        nx, ny = x + dx, y + dy
                        if 0 <= nx < width and 0 <= ny < height:
                            surrounding_depth = node.depth_image[ny, nx]
                            if node.depth_image.dtype == np.uint16:
                                surrounding_depth_m = float(surrounding_depth) / 1000.0
                            elif node.depth_image.dtype == np.float32:
                                surrounding_depth_m = float(surrounding_depth)
                            else:
                                surrounding_depth_m = float(surrounding_depth) / 1000.0
                            surrounding_depths.append({
                                "x": nx, "y": ny, 
                                "depth_raw": float(surrounding_depth),
                                "depth_meters": surrounding_depth_m,
                                "valid": surrounding_depth > 0
                            })
                
                # 清理节点
                node.destroy_node()
                
                return True, "深度信息获取成功 Depth information retrieved successfully", {
                    "x": x,
                    "y": y,
                    "depth_raw": float(depth_value),
                    "depth_meters": depth_meters,
                    "depth_millimeters": depth_mm,
                    "is_valid": is_valid,
                    "image_size": {"width": width, "height": height},
                    "data_type": str(node.depth_image.dtype),
                    "surrounding_area": surrounding_depths,
                    "method": "cv_bridge_depth"
                }
            else:
                # 深度图像获取超时 Depth image capture timeout
                # 请检查深度相机是否正常发布数据 Please check if depth camera is publishing data normally
                node.destroy_node()
                return False, f"深度图像获取超时 Depth image capture timeout ({timeout_s}s)", {
                    "x": x, "y": y, 
                    "timeout": timeout_s, 
                    "suggestion": "check_depth_camera_publishing"
                }
                
        except Exception as e:
            # 深度信息获取过程中发生错误 Error during depth information retrieval
            return False, f"深度信息获取过程中发生错误 Error during depth information retrieval: {str(e)}", {
                "x": x, "y": y, 
                "error": str(e)
            }
    
    # ==================== 雷达相关功能 Lidar Related Functions ====================
    
    def _get_lidar_type(self) -> str:
        """
        获取当前雷达类型
        Get current lidar type from environment variable
        
        Returns:
            str: 雷达类型 ('a1' 或 '4ROS') Lidar type ('a1' or '4ROS')
        """
        lidar_type = os.environ.get('RPLIDAR_TYPE', 'a1').lower()
        return lidar_type if lidar_type in ['a1', '4ros'] else 'a1'
    
    def start_lidar(self, wait_time_s: float = 5.0, check_before_start: bool = True) -> tuple:
        """
        启动雷达节点
        Start lidar node
        
        Args:
            wait_time_s: 启动后等待时间(秒) Wait time after start in seconds
            check_before_start: 启动前检查是否已运行 Check if already running before start
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 启动是否成功 Whether start was successful
                message: 启动结果消息 Start result message
                data: 启动状态信息 Start status information
        
        Examples:
            success, msg, data = sensor_manager.start_lidar()
        """
        try:
            lidar_type = self._get_lidar_type()
            
            # 检查是否已经运行
            if check_before_start:
                node_name = "sllidar_node" if lidar_type == "a1" else "ydlidar_ros2_driver_node"
                success, msg, is_running = self.check_ros2_node_running(node_name)
                if success and is_running:
                    return True, f"雷达节点已在运行 Lidar node already running: {lidar_type}", {
                        "lidar_type": lidar_type,
                        "node_name": node_name,
                        "already_running": True
                    }
            
            # 根据雷达类型选择启动命令
            if lidar_type == "a1":
                cmd = ["ros2", "launch", "sllidar_ros2", "sllidar_launch.py"]
                node_name = "sllidar_node"
                topic_name = "/scan"
            else:  # 4ROS
                cmd = ["ros2", "launch", "ydlidar_ros2_driver", "ydlidar_raw_launch.py"]
                node_name = "ydlidar_ros2_driver_node"
                topic_name = "/scan"
            
            # 启动雷达节点
            process = subprocess.Popen(
                cmd,
                stdout=subprocess.PIPE,
                stderr=subprocess.PIPE,
                text=True
            )
            
            # 等待启动
            time.sleep(wait_time_s)
            
            # 检查节点是否成功启动
            success, msg, is_running = self.check_ros2_node_running(node_name)
            if success and is_running:
                # 检查话题是否正常发布
                topic_success, topic_msg, topic_data = self.check_ros2_topic_active(topic_name, "sensor_msgs/msg/LaserScan")
                
                return True, f"雷达节点启动成功 Lidar node started successfully: {lidar_type}", {
                    "lidar_type": lidar_type,
                    "node_name": node_name,
                    "topic_name": topic_name,
                    "process_id": process.pid,
                    "node_running": True,
                    "topic_active": topic_data.get('active', False) if topic_success else False
                }
            else:
                return False, f"雷达节点启动失败 Failed to start lidar node: {lidar_type}", {
                    "lidar_type": lidar_type,
                    "node_name": node_name,
                    "process_id": process.pid,
                    "node_running": False,
                    "error": msg
                }
                
        except FileNotFoundError:
            return False, "ROS2未安装或雷达驱动包未找到 ROS2 not installed or lidar driver package not found", {
                "lidar_type": lidar_type,
                "error": "ros2_or_driver_not_found"
            }
        except Exception as e:
            return False, f"启动雷达节点出错 Error starting lidar node: {str(e)}", {
                "lidar_type": lidar_type,
                "error": str(e)
            }
    
    def get_lidar_data(self, timeout_s: float = 5.0, sample_count: int = 1) -> tuple:
        """
        读取雷达节点数据
        Read lidar node data
        
        Args:
            timeout_s: 读取超时时间(秒) Read timeout in seconds
            sample_count: 采样次数 Number of samples to collect
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 读取是否成功 Whether read was successful
                message: 读取结果消息 Read result message
                data: 雷达数据信息 Lidar data information
        
        Examples:
            success, msg, data = sensor_manager.get_lidar_data()
        """
        try:
            lidar_type = self._get_lidar_type()
            topic_name = "/scan"
            
            # 检查话题是否活跃
            topic_success, topic_msg, topic_data = self.check_ros2_topic_active(topic_name, "sensor_msgs/msg/LaserScan")
            if not topic_success or not topic_data.get('active', False):
                return False, f"雷达话题未活跃 Lidar topic not active: {topic_name}", {
                    "lidar_type": lidar_type,
                    "topic_name": topic_name,
                    "topic_active": False,
                    "error": topic_msg
                }
            
            # 读取雷达数据 - 兼容ROS2 Foxy版本，使用非阻塞方式
            cmd = ["ros2", "topic", "echo", topic_name]
            
            try:
                # 使用Popen启动进程
                process = subprocess.Popen(
                    cmd,
                    stdout=subprocess.PIPE,
                    stderr=subprocess.PIPE,
                    text=True
                )
                
                # 等待数据输出，最多等待指定时间
                output_lines = []
                start_time = time.time()
                ranges_found = False
                ranges_complete = False
                
                while (time.time() - start_time) < timeout_s:
                    # 非阻塞读取一行
                    try:
                        line = process.stdout.readline()
                        if line:
                            line_stripped = line.strip()
                            output_lines.append(line_stripped)
                            
                            # 检测ranges数组开始
                            if line_stripped == "ranges:":
                                ranges_found = True
                            # 检测ranges数组结束
                            elif ranges_found and (line_stripped == "intensities:" or 
                                                  (line_stripped.endswith(":") and not line_stripped.startswith("- "))):
                                ranges_complete = True
                                break
                            elif ranges_complete and len(output_lines) > 50:
                                break
                        else:
                            time.sleep(0.05)  # 短暂等待，减少CPU占用
                    except:
                        break
                
                # 终止进程
                try:
                    process.terminate()
                    process.wait(timeout=1.0)
                except Exception as e:
                    try:
                        process.kill()
                        process.wait(timeout=1.0)
                    except:
                        pass
                
                if output_lines:
                    output = '\n'.join(output_lines)
                    # 解析基本信息
                    scan_info = {
                        "lidar_type": lidar_type,
                        "topic_name": topic_name,
                        "data_received": True,
                        "raw_data_length": len(output),
                        "timestamp": time.time(),
                        "lines_received": len(output_lines)
                    }
                    
                    # 解析LaserScan消息的各个字段
                    ranges_data = []
                    intensities_data = []
                    in_ranges_section = False
                    in_intensities_section = False
                    
                    for line in output_lines:
                        line = line.strip()
                        try:
                            # 解析header信息
                            if "frame_id:" in line:
                                scan_info["frame_id"] = line.split(":")[1].strip()
                            # 解析角度信息
                            elif "angle_min:" in line:
                                scan_info["angle_min"] = float(line.split(":")[1].strip())
                            elif "angle_max:" in line:
                                scan_info["angle_max"] = float(line.split(":")[1].strip())
                            elif "angle_increment:" in line:
                                scan_info["angle_increment"] = float(line.split(":")[1].strip())
                            # 解析时间信息
                            elif "time_increment:" in line:
                                scan_info["time_increment"] = float(line.split(":")[1].strip())
                            elif "scan_time:" in line:
                                scan_info["scan_time"] = float(line.split(":")[1].strip())
                            # 解析距离范围
                            elif "range_min:" in line:
                                scan_info["range_min"] = float(line.split(":")[1].strip())
                            elif "range_max:" in line:
                                scan_info["range_max"] = float(line.split(":")[1].strip())
                            # 检测ranges数组开始
                            elif line == "ranges:":
                                in_ranges_section = True
                                in_intensities_section = False
                            # 检测intensities数组开始
                            elif line == "intensities:":
                                in_intensities_section = True
                                in_ranges_section = False
                            # 解析ranges数据
                            elif in_ranges_section and line.startswith("- "):
                                try:
                                    value_str = line[2:].strip()  # 移除"- "前缀
                                    if value_str == ".inf":
                                        ranges_data.append(float('inf'))
                                    elif value_str == "-.inf":
                                        ranges_data.append(float('-inf'))
                                    elif value_str == "nan":
                                        ranges_data.append(float('nan'))
                                    else:
                                        ranges_data.append(float(value_str))
                                except ValueError:
                                    continue
                            # 解析intensities数据
                            elif in_intensities_section and line.startswith("- "):
                                try:
                                    value_str = line[2:].strip()  # 移除"- "前缀
                                    intensities_data.append(float(value_str))
                                except ValueError:
                                    continue
                            # 检测到其他字段时停止当前数组解析
                            elif line.endswith(":") and not line.startswith("- "):
                                in_ranges_section = False
                                in_intensities_section = False
                        except (ValueError, IndexError):
                            continue  # 忽略解析错误的行
                    
                    # 添加解析到的数组数据
                    if ranges_data:
                        scan_info["ranges"] = ranges_data
                        scan_info["ranges_count"] = len(ranges_data)
                        # 计算有效距离数据统计
                        valid_ranges = [r for r in ranges_data if not (r == float('inf') or r == float('-inf') or r != r)]  # 排除inf和nan
                        if valid_ranges:
                            scan_info["valid_ranges_count"] = len(valid_ranges)
                            scan_info["min_range"] = min(valid_ranges)
                            scan_info["max_range"] = max(valid_ranges)
                            scan_info["avg_range"] = sum(valid_ranges) / len(valid_ranges)
                        else:
                            scan_info["valid_ranges_count"] = 0
                    
                    if intensities_data:
                        scan_info["intensities"] = intensities_data
                        scan_info["intensities_count"] = len(intensities_data)
                    
                    # 生成sample_points（角度-距离对）
                    if "angle_min" in scan_info and "angle_increment" in scan_info and ranges_data:
                        sample_points = []
                        angle_min = scan_info["angle_min"]
                        angle_increment = scan_info["angle_increment"]
                        
                        for i, range_val in enumerate(ranges_data):
                            angle = angle_min + i * angle_increment
                            sample_points.append({
                                "angle": angle,
                                "distance": range_val,
                                "intensity": intensities_data[i] if i < len(intensities_data) else 0.0
                            })
                        
                        scan_info["sample_points"] = sample_points
                    
                    return True, f"成功读取雷达数据 Successfully read lidar data: {lidar_type}", scan_info
                else:
                    return False, f"雷达数据为空 Empty lidar data: {topic_name}", {
                        "lidar_type": lidar_type,
                        "topic_name": topic_name,
                        "data_received": False,
                        "error": "empty_data"
                    }
                    
            except Exception as e:
                return False, f"读取雷达数据出错 Error reading lidar data: {str(e)}", {
                    "lidar_type": lidar_type,
                    "topic_name": topic_name,
                    "data_received": False,
                    "error": str(e)
                }
                
        except subprocess.TimeoutExpired:
            return False, f"读取雷达数据超时 Lidar data read timeout: {timeout_s}s", {
                "lidar_type": lidar_type,
                "topic_name": topic_name,
                "data_received": False,
                "error": "timeout"
            }
        except Exception as e:
            return False, f"读取雷达数据出错 Error reading lidar data: {str(e)}", {
                "lidar_type": lidar_type,
                "error": str(e)
            }
    
    def check_lidar_status(self) -> tuple:
        """
        检查雷达状态
        Check lidar status
        
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 检查是否成功 Whether check was successful
                message: 检查结果消息 Check result message
                data: 雷达状态信息 Lidar status information
        """
        try:
            lidar_type = self._get_lidar_type()
            node_name = "sllidar_node" if lidar_type == "a1" else "ydlidar_ros2_driver_node"
            topic_name = "/scan"
            
            # 检查雷达节点状态
            node_success, node_msg, node_running = self.check_ros2_node_running(node_name)
            
            # 检查雷达话题状态
            topic_success, topic_msg, topic_data = self.check_ros2_topic_active(topic_name, "sensor_msgs/msg/LaserScan")
            topic_active = topic_success and topic_data.get('active', False)
            
            # 确定综合状态
            if node_running and topic_active:
                overall_status = "normal"
                status = "running"
            elif node_running and not topic_active:
                overall_status = "partial"
                status = "partial"
            else:
                overall_status = "unavailable"
                status = "stopped"
            
            # 构建返回数据
            result_data = {
                "status": status,
                "overall_status": overall_status,
                "lidar_type": lidar_type,
                "node_name": node_name,
                "topic_name": topic_name,
                "node_running": node_running,
                "topic_active": topic_active,
                "node_check": {"success": node_success, "message": node_msg},
                "topic_check": {"success": topic_success, "message": topic_msg, "data": topic_data}
            }
            
            # 添加详细的检查消息
            if node_success:
                result_data["node_check_message"] = node_msg
            else:
                result_data["node_check_error"] = node_msg
                
            if topic_success:
                result_data["topic_check_message"] = topic_msg
                if topic_data and "publisher_count" in topic_data:
                    result_data["publisher_count"] = topic_data["publisher_count"]
            else:
                result_data["topic_check_error"] = topic_msg
            
            return True, f"雷达状态: {status} Lidar status: {status}", result_data
        except Exception as e:
            return False, f"检查雷达状态出错 Error checking lidar status: {str(e)}", {"error": str(e)}
    
    def stop_lidar(self, force_kill: bool = False, wait_time_s: float = 3.0) -> tuple:
        """
        停止雷达
        Stop lidar
        
        Args:
            force_kill: 是否强制终止 Whether to force kill
            wait_time_s: 等待时间(秒) Wait time in seconds
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 停止是否成功 Whether stop was successful
                message: 停止结果消息 Stop result message
                data: 停止状态信息 Stop status information
        """
        try:
            lidar_type = self._get_lidar_type()
            node_name = "sllidar_node" if lidar_type == "a1" else "ydlidar_ros2_driver_node"
            
            # 检查雷达是否在运行
            node_success, node_msg, node_running = self.check_ros2_node_running(node_name)
            
            if not node_running:
                return True, "雷达已经停止 Lidar already stopped", {
                    "status": "already_stopped",
                    "lidar_type": lidar_type,
                    "node_running": False
                }
            
            # 强制终止雷达进程
            try:
                # 查找并终止雷达相关进程
                if lidar_type == "a1":
                    kill_pattern = "sllidar"
                else:
                    kill_pattern = "ydlidar"
                
                result = subprocess.run(
                    ["pkill", "-f", kill_pattern],
                    capture_output=True,
                    text=True,
                    timeout=5.0
                )
                
                time.sleep(wait_time_s)
                
                # 检查节点是否已停止
                node_success, node_msg, node_running = self.check_ros2_node_running(node_name)
                
                if not node_running:
                    return True, f"雷达停止成功 Lidar stopped successfully: {lidar_type}", {
                        "method": "pkill",
                        "lidar_type": lidar_type,
                        "node_running": False
                    }
                else:
                    return False, f"雷达停止失败 Failed to stop lidar: {lidar_type}", {
                        "method": "pkill",
                        "lidar_type": lidar_type,
                        "node_running": True,
                        "error": "still_running"
                    }
                    
            except Exception as e:
                return False, f"停止雷达出错 Error stopping lidar: {str(e)}", {
                    "lidar_type": lidar_type,
                    "error": str(e)
                }
                
        except Exception as e:
            return False, f"停止雷达出错 Error stopping lidar: {str(e)}", {
                "lidar_type": lidar_type,
                "error": str(e)
            }
    
    # ==================== 位置传感器功能 Position Sensor Functions ====================
    
    def get_robot_position_in_map(self, timeout_s: float = 10.0) -> tuple:
        """
        获取机器人在地图中的位置 (使用tf2_echo)
        Get robot position in map (using tf2_echo)
        
        Args:
            timeout_s: 超时时间(秒) Timeout in seconds
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 获取是否成功 Whether get was successful
                message: 获取结果消息 Get result message
                data: 位置信息 Position information
                    {
                        "position": {"x": float, "y": float, "z": float},
                        "orientation": {"x": float, "y": float, "z": float, "w": float},
                        "frame_id": str,
                        "child_frame_id": str,
                        "timestamp": float
                    }
        
        Examples:
            success, msg, position = sensor_manager.get_robot_position_in_map()
        """
        try:
            # 检查ROS2环境 - 使用ros2 node list命令验证
            ros2_check = subprocess.run(
                ["ros2", "node", "list"],
                capture_output=True,
                text=True,
                timeout=3.0
            )
            
            if ros2_check.returncode != 0:
                return False, "ROS2环境不可用 ROS2 environment not available", {}
            
            # 多次检验map相关话题的存在 - Multiple verification of map-related topics
            print("🔍 开始检验map相关话题 Starting map topic verification...")
            
            # 检验/tf话题 - Check /tf topic
            max_retries = 3
            retry_delay = 1.0
            
            for attempt in range(max_retries):
                print(f"📡 检验/tf话题 (尝试 {attempt + 1}/{max_retries}) Checking /tf topic (attempt {attempt + 1}/{max_retries})")
                
                try:
                    tf_check = subprocess.run(
                        ["ros2", "topic", "info", "/tf", "--verbose"],
                        capture_output=True,
                        text=True,
                        timeout=5.0
                    )
                    
                    if tf_check.returncode == 0 and "tf2_msgs/msg/TFMessage" in tf_check.stdout:
                        # 检查发布者数量
                        publisher_count = 0
                        if "Publisher count:" in tf_check.stdout:
                            for line in tf_check.stdout.split('\n'):
                                if "Publisher count:" in line:
                                    publisher_count = int(line.split(":")[1].strip())
                                    break
                        
                        if publisher_count > 0:
                            print(f"✅ /tf话题有发布者 /tf topic has publishers (count: {publisher_count})")
                            
                            # 进一步检查是否有实际数据发布 - Check if there's actual data being published
                            print(f"🔍 检查/tf话题数据发布情况 Checking /tf topic data publishing...")
                            try:
                                # 使用ros2 topic echo检查是否有数据发布，设置短超时
                                data_check = subprocess.run(
                                    ["ros2", "topic", "echo", "/tf", "--once"],
                                    capture_output=True,
                                    text=True,
                                    timeout=3.0  # 短超时，避免长时间等待
                                )
                                
                                if data_check.returncode == 0 and data_check.stdout.strip():
                                    print(f"✅ /tf话题正常发布数据 /tf topic actively publishing data")
                                    break
                                else:
                                    print(f"⚠️ /tf话题有发布者但无数据发布 /tf topic has publishers but no data published")
                                    if attempt < max_retries - 1:
                                        print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                        time.sleep(retry_delay)
                                        continue
                                    else:
                                        return False, "map相关话题检验失败: /tf话题有发布者但无数据发布，可能导致tf2_echo卡死 Map topic verification failed: /tf topic has publishers but no data published, may cause tf2_echo to hang", {}
                            
                            except subprocess.TimeoutExpired:
                                print(f"⚠️ /tf话题数据检查超时，可能无数据发布 /tf topic data check timeout, possibly no data published")
                                if attempt < max_retries - 1:
                                    print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                    time.sleep(retry_delay)
                                    continue
                                else:
                                    return False, "map相关话题检验失败: /tf话题数据检查超时，可能无数据发布，会导致tf2_echo卡死 Map topic verification failed: /tf topic data check timeout, possibly no data published, will cause tf2_echo to hang", {}
                            
                            except Exception as data_e:
                                print(f"❌ /tf话题数据检查出错 /tf topic data check error: {data_e}")
                                if attempt < max_retries - 1:
                                    print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                    time.sleep(retry_delay)
                                    continue
                                else:
                                    return False, f"map相关话题检验失败: /tf话题数据检查出错 Map topic verification failed: /tf topic data check error: {str(data_e)}", {}
                        
                        else:
                            print(f"❌ /tf话题无发布者 /tf topic has no publishers")
                            if attempt < max_retries - 1:
                                print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                time.sleep(retry_delay)
                                continue
                            else:
                                return False, "map相关话题检验失败: /tf话题无发布者 Map topic verification failed: /tf topic has no publishers", {}
                        break
                    else:
                        print(f"❌ /tf话题不可用 /tf topic not available")
                        if attempt < max_retries - 1:
                            print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                            time.sleep(retry_delay)
                            continue
                        else:
                            return False, "map相关话题检验失败: /tf话题不可用 Map topic verification failed: /tf topic not available", {}
                            
                except subprocess.TimeoutExpired:
                    print(f"⏰ /tf话题检查超时 /tf topic check timeout")
                    if attempt < max_retries - 1:
                        print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                        time.sleep(retry_delay)
                        continue
                    else:
                        return False, "map相关话题检验失败: /tf话题检查超时 Map topic verification failed: /tf topic check timeout", {}
                except Exception as e:
                    print(f"❌ /tf话题检查出错 /tf topic check error: {e}")
                    if attempt < max_retries - 1:
                        print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                        time.sleep(retry_delay)
                        continue
                    else:
                        return False, f"map相关话题检验失败: /tf话题检查出错 Map topic verification failed: /tf topic check error: {str(e)}", {}
            
            # 检验/tf_static话题 - Check /tf_static topic
            for attempt in range(max_retries):
                print(f"📡 检验/tf_static话题 (尝试 {attempt + 1}/{max_retries}) Checking /tf_static topic (attempt {attempt + 1}/{max_retries})")
                
                try:
                    tf_static_check = subprocess.run(
                        ["ros2", "topic", "info", "/tf_static", "--verbose"],
                        capture_output=True,
                        text=True,
                        timeout=5.0
                    )
                    
                    if tf_static_check.returncode == 0 and "tf2_msgs/msg/TFMessage" in tf_static_check.stdout:
                        # 检查发布者数量
                        publisher_count = 0
                        if "Publisher count:" in tf_static_check.stdout:
                            for line in tf_static_check.stdout.split('\n'):
                                if "Publisher count:" in line:
                                    publisher_count = int(line.split(":")[1].strip())
                                    break
                        
                        if publisher_count > 0:
                            print(f"✅ /tf_static话题有发布者 /tf_static topic has publishers (count: {publisher_count})")
                            
                            # 进一步检查是否有实际数据发布 - Check if there's actual data being published
                            print(f"🔍 检查/tf_static话题数据发布情况 Checking /tf_static topic data publishing...")
                            try:
                                # 使用ros2 topic echo检查是否有数据发布，设置短超时
                                data_check = subprocess.run(
                                    ["ros2", "topic", "echo", "/tf_static", "--once"],
                                    capture_output=True,
                                    text=True,
                                    timeout=3.0  # 短超时，避免长时间等待
                                )
                                
                                if data_check.returncode == 0 and data_check.stdout.strip():
                                    print(f"✅ /tf_static话题正常发布数据 /tf_static topic actively publishing data")
                                    break
                                else:
                                    print(f"⚠️ /tf_static话题有发布者但无数据发布 /tf_static topic has publishers but no data published")
                                    if attempt < max_retries - 1:
                                        print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                        time.sleep(retry_delay)
                                        continue
                                    else:
                                        return False, "map相关话题检验失败: /tf_static话题有发布者但无数据发布，可能导致tf2_echo卡死 Map topic verification failed: /tf_static topic has publishers but no data published, may cause tf2_echo to hang", {}
                            
                            except subprocess.TimeoutExpired:
                                print(f"⚠️ /tf_static话题数据检查超时，可能无数据发布 /tf_static topic data check timeout, possibly no data published")
                                if attempt < max_retries - 1:
                                    print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                    time.sleep(retry_delay)
                                    continue
                                else:
                                    return False, "map相关话题检验失败: /tf_static话题数据检查超时，可能无数据发布，会导致tf2_echo卡死 Map topic verification failed: /tf_static topic data check timeout, possibly no data published, will cause tf2_echo to hang", {}
                            
                            except Exception as data_e:
                                print(f"❌ /tf_static话题数据检查出错 /tf_static topic data check error: {data_e}")
                                if attempt < max_retries - 1:
                                    print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                    time.sleep(retry_delay)
                                    continue
                                else:
                                    return False, f"map相关话题检验失败: /tf_static话题数据检查出错 Map topic verification failed: /tf_static topic data check error: {str(data_e)}", {}
                        
                        else:
                            print(f"❌ /tf_static话题无发布者 /tf_static topic has no publishers")
                            if attempt < max_retries - 1:
                                print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                time.sleep(retry_delay)
                                continue
                            else:
                                return False, "map相关话题检验失败: /tf_static话题无发布者 Map topic verification failed: /tf_static topic has no publishers", {}
                        break
                    else:
                        print(f"❌ /tf_static话题不可用 /tf_static topic not available")
                        if attempt < max_retries - 1:
                            print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                            time.sleep(retry_delay)
                            continue
                        else:
                            return False, "map相关话题检验失败: /tf_static话题不可用 Map topic verification failed: /tf_static topic not available", {}
                            
                except subprocess.TimeoutExpired:
                    print(f"⏰ /tf_static话题检查超时 /tf_static topic check timeout")
                    if attempt < max_retries - 1:
                        print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                        time.sleep(retry_delay)
                        continue
                    else:
                        return False, "map相关话题检验失败: /tf_static话题检查超时 Map topic verification failed: /tf_static topic check timeout", {}
                except Exception as e:
                    print(f"❌ /tf_static话题检查出错 /tf_static topic check error: {e}")
                    if attempt < max_retries - 1:
                        print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                        time.sleep(retry_delay)
                        continue
                    else:
                        return False, f"map相关话题检验失败: /tf_static话题检查出错 Map topic verification failed: /tf_static topic check error: {str(e)}", {}
            
            # 检验/map话题 - Check /map topic
            for attempt in range(max_retries):
                print(f"📡 检验/map话题 (尝试 {attempt + 1}/{max_retries}) Checking /map topic (attempt {attempt + 1}/{max_retries})")
                
                try:
                    map_check = subprocess.run(
                        ["ros2", "topic", "info", "/map", "--verbose"],
                        capture_output=True,
                        text=True,
                        timeout=5.0
                    )
                    
                    if map_check.returncode == 0 and "nav_msgs/msg/OccupancyGrid" in map_check.stdout:
                        # 检查发布者数量
                        publisher_count = 0
                        if "Publisher count:" in map_check.stdout:
                            for line in map_check.stdout.split('\n'):
                                if "Publisher count:" in line:
                                    publisher_count = int(line.split(":")[1].strip())
                                    break
                        
                        if publisher_count > 0:
                            print(f"✅ /map话题有发布者 /map topic has publishers (count: {publisher_count})")
                            
                            # 进一步检查是否有实际数据发布 - Check if there's actual data being published
                            print(f"🔍 检查/map话题数据发布情况 Checking /map topic data publishing...")
                            try:
                                # 使用ros2 topic echo检查是否有数据发布，设置短超时
                                data_check = subprocess.run(
                                    ["ros2", "topic", "echo", "/map", "--once"],
                                    capture_output=True,
                                    text=True,
                                    timeout=3.0  # 短超时，避免长时间等待
                                )
                                
                                if data_check.returncode == 0 and data_check.stdout.strip():
                                    print(f"✅ /map话题正常发布数据 /map topic actively publishing data")
                                    break
                                else:
                                    print(f"⚠️ /map话题有发布者但无数据发布 /map topic has publishers but no data published")
                                    if attempt < max_retries - 1:
                                        print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                        time.sleep(retry_delay)
                                        continue
                                    else:
                                        return False, "map相关话题检验失败: /map话题有发布者但无数据发布，可能导致tf2_echo卡死 Map topic verification failed: /map topic has publishers but no data published, may cause tf2_echo to hang", {}
                            
                            except subprocess.TimeoutExpired:
                                print(f"⚠️ /map话题数据检查超时，可能无数据发布 /map topic data check timeout, possibly no data published")
                                if attempt < max_retries - 1:
                                    print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                    time.sleep(retry_delay)
                                    continue
                                else:
                                    return False, "map相关话题检验失败: /map话题数据检查超时，可能无数据发布，会导致tf2_echo卡死 Map topic verification failed: /map topic data check timeout, possibly no data published, will cause tf2_echo to hang", {}
                            
                            except Exception as data_e:
                                print(f"❌ /map话题数据检查出错 /map topic data check error: {data_e}")
                                if attempt < max_retries - 1:
                                    print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                    time.sleep(retry_delay)
                                    continue
                                else:
                                    return False, f"map相关话题检验失败: /map话题数据检查出错 Map topic verification failed: /map topic data check error: {str(data_e)}", {}
                        
                        else:
                            print(f"❌ /map话题无发布者 /map topic has no publishers")
                            if attempt < max_retries - 1:
                                print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                                time.sleep(retry_delay)
                                continue
                            else:
                                return False, "map相关话题检验失败: /map话题无发布者 Map topic verification failed: /map topic has no publishers", {}
                    else:
                        print(f"❌ /map话题不可用 /map topic not available")
                        if attempt < max_retries - 1:
                            print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                            time.sleep(retry_delay)
                            continue
                        else:
                            return False, "map相关话题检验失败: /map话题不可用 Map topic verification failed: /map topic not available", {}
                            
                except subprocess.TimeoutExpired:
                    print(f"⏰ /map话题检查超时 /map topic check timeout")
                    if attempt < max_retries - 1:
                        print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                        time.sleep(retry_delay)
                        continue
                    else:
                        return False, "map相关话题检验失败: /map话题检查超时 Map topic verification failed: /map topic check timeout", {}
                except Exception as e:
                    print(f"❌ /map话题检查出错 /map topic check error: {e}")
                    if attempt < max_retries - 1:
                        print(f"⏳ 等待{retry_delay}秒后重试 Waiting {retry_delay}s before retry...")
                        time.sleep(retry_delay)
                        continue
                    else:
                        return False, f"map相关话题检验失败: /map话题检查出错 Map topic verification failed: /map topic check error: {str(e)}", {}
            
            print("✅ map相关话题检验通过 Map topic verification passed")
            
            # 使用tf2_echo命令直接获取map到base_link的变换
            cmd = [
                "ros2", "run", "tf2_ros", "tf2_echo", "map", "base_link"
            ]
            
            # 执行tf2_echo命令 - 使用Popen因为tf2_echo是持续运行的命令
            import signal
            process = subprocess.Popen(
                cmd,
                stdout=subprocess.PIPE,
                stderr=subprocess.PIPE,
                text=True,
                preexec_fn=os.setsid if hasattr(os, 'setsid') else None
            )
            
            try:
                # 读取输出直到获取到有效的变换数据
                tf_output = ""
                start_time = time.time()
                
                while time.time() - start_time < timeout_s:
                    # 检查进程是否还在运行
                    if process.poll() is not None:
                        # 进程已结束，检查错误
                        stderr_output = process.stderr.read()
                        if stderr_output:
                            return False, f"无法获取map到base_link的变换 Cannot get map to base_link transform: {stderr_output}", {}
                        break
                    
                    # 尝试读取一行输出
                    try:
                        line = process.stdout.readline()
                        if line:
                            tf_output += line
                            # 检查是否已经获取到完整的变换数据
                            if "- Translation:" in tf_output and "- Rotation:" in tf_output:
                                # 获取到完整数据，终止进程
                                break
                    except:
                        time.sleep(0.1)
                        continue
                    
                    time.sleep(0.1)
                
            finally:
                # 确保进程被正确终止
                try:
                    if hasattr(os, 'killpg'):
                        os.killpg(os.getpgid(process.pid), signal.SIGTERM)
                    else:
                        process.terminate()
                    process.wait(timeout=2)
                except:
                    try:
                        if hasattr(os, 'killpg'):
                            os.killpg(os.getpgid(process.pid), signal.SIGKILL)
                        else:
                            process.kill()
                    except:
                        pass
            
            if not tf_output or not tf_output.strip():
                return False, "tf2_echo输出为空 tf2_echo output is empty", {}
            
            # 解析tf2_echo的输出
            tf_output = tf_output.strip()
            
            # 解析输出数据
            lines = tf_output.split('\n')
            position = {"x": 0.0, "y": 0.0, "z": 0.0}
            orientation = {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
            
            for line in lines:
                line = line.strip()
                
                # 解析translation数据 - 格式: - Translation: [x, y, z]
                if "- Translation:" in line and "[" in line and "]" in line:
                    try:
                        coords_str = line.split("[")[1].split("]")[0]
                        coords = [float(x.strip()) for x in coords_str.split(",")]
                        if len(coords) >= 3:
                            position["x"] = coords[0]
                            position["y"] = coords[1]
                            position["z"] = coords[2]
                    except (ValueError, IndexError):
                        pass
                
                # 解析rotation数据 - 格式: - Rotation: in Quaternion [x, y, z, w]
                elif "- Rotation:" in line and "Quaternion" in line and "[" in line and "]" in line:
                    try:
                        coords_str = line.split("[")[1].split("]")[0]
                        coords = [float(x.strip()) for x in coords_str.split(",")]
                        if len(coords) >= 4:
                            orientation["x"] = coords[0]
                            orientation["y"] = coords[1]
                            orientation["z"] = coords[2]
                            orientation["w"] = coords[3]
                    except (ValueError, IndexError):
                        pass
            
            # 构建返回数据
            transform_data = {
                "position": position,
                "orientation": orientation,
                "frame_id": "map",
                "child_frame_id": "base_link",
                "timestamp": time.time()
            }
            
            return True, "成功获取机器人位置 Successfully retrieved robot position", transform_data
            
        except subprocess.TimeoutExpired:
            return False, f"获取位置信息超时 Position data timeout: {timeout_s}s", {}
        except Exception as e:
            return False, f"获取机器人位置出错 Error getting robot position: {str(e)}", {}
    
    def print_system_info(self) -> tuple:
        """
        打印完整系统信息
        Print complete system information
        
        Returns:
            tuple: (success: bool, message: str, data: dict)
        """
        try:
            print("\n=== 机器人系统信息 Robot System Information ===")
            
            # 1. ROS2节点状态
            print("\n🔧 ROS2节点状态:")
            nodes_to_check = [
                "camera_node",
                "lidar_node", 
                "navigation_node",
                "tf2_ros"
            ]
            
            for node in nodes_to_check:
                success, message, data = self.check_ros2_node_running(node)
                status = "✅ 运行中" if success else "❌ 未运行"
                print(f"  {node}: {status}")
            
            # 2. 相机状态
            print("\n📷 相机状态:")
            success, message, data = self.get_camera_status()
            if success and data:
                print(f"  状态: {data.get('status', 'Unknown')}")
                print(f"  话题: {data.get('topics', [])}")
            else:
                print(f"  状态: 获取失败 - {message}")
            
            # 3. 激光雷达状态
            print("\n🔍 激光雷达状态:")
            success, message, data = self.check_lidar_status()
            if success and data:
                print(f"  状态: {data.get('status', 'Unknown')}")
                print(f"  话题: {data.get('topic', 'Unknown')}")
            else:
                print(f"  状态: 获取失败 - {message}")
            
            # 4. 机器人位置信息
            print("\n🗺️ 机器人位置:")
            success, message, data = self.get_robot_position_in_map()
            if success and data:
                pos = data.get('position', {})
                orient = data.get('orientation', {})
                print(f"  位置: x={pos.get('x', 0):.3f}, y={pos.get('y', 0):.3f}, z={pos.get('z', 0):.3f}")
                print(f"  姿态: x={orient.get('x', 0):.3f}, y={orient.get('y', 0):.3f}, z={orient.get('z', 0):.3f}, w={orient.get('w', 0):.3f}")
            else:
                print(f"  位置: 获取失败 - {message}")
            
            print("\n✅ 系统信息打印完成")
            
            return True, "系统信息打印成功 System information printed successfully", {
                'timestamp': time.time(),
                'info_printed': True
            }
            
        except Exception as e:
            error_msg = f"打印系统信息失败 Failed to print system info: {str(e)}"
            print(f"\n❌ {error_msg}")
            return False, error_msg, None